﻿// Inner Fire 游戏引擎库
// RigidBody - 刚体
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 对应Unity中的刚体插件。
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-17

#include "rigid_body.h"
#include <game/object/game_object.h>
#include <game/physics/collision.h>
#include <klog.h>

using namespace DirectX;

namespace ifire::game {

RigidBody::RigidBody(GameObject* game_object)
    : game_object_(game_object)
    , velocity_(0.0f, 0.0f, 0.0f)
    , acceleration_(0.0f, 0.0f, 0.0f)
    , mass_(1.0f)
    , angular_velocity_(0.0f, 0.0f, 0.0f)
    , last_velocity_(0.0f, 0.0f, 0.0f)
    , last_angular_velocity_(0.0f, 0.0f, 0.0f)
    , friction_(0.3f)
    , restitution_(0.5f) {
  // 创建碰撞体（这个函数拥有检查机制，不会重复创建）
  collider_ = game_object_->AddCollider();

  // 设置Mass
  SetMass(mass_);

  // 继承GameObject的项目，一但使用RigidBody，则表示接管GameObject的Transform，
  // 所有的项目都由Rigid改变。
  auto& t = GetTransform();
  position_ = t.position;
  rotation_ = t.rotation;
  scale_ = t.scale;

  // 默认设置Particle，并且触发惯性张量/Box的更新
  SetParticle();
}

void RigidBody::SetParticle() {
  collider_->SetParticle();
  MarkBoundingDirty();
  BuildLocalInertia();
}

void RigidBody::SetSphere(float radius) {
  collider_->SetSphere(radius);
  MarkBoundingDirty();
  BuildLocalInertia();
}

void RigidBody::SetBox(XMFLOAT3 box) {
  collider_->SetBox(box);
  MarkBoundingDirty();
  BuildLocalInertia();
}

void RigidBody::SetCapsule(float radius, float start, float end) {
  collider_->SetCapsule(radius, start, end);
  MarkBoundingDirty();
  BuildLocalInertia();
}

bool RigidBody::IsStatic() const {
  // 比较每个分量是否小于 epsilon;
  return inv_mass_ < FLT_EPSILON;
}

void RigidBody::SetPosition(IVec3 value) {
  position_ = value;
  MarkBoundingDirty();
}

IVec3 RigidBody::GetPosition() const { return position_; }

void RigidBody::SetRotationEuler(IVec3 value) {
  rotation_ = imath::EulerToQuaternion(value);
  MarkInertiaDirty();
  MarkBoundingDirty();
}

IVec3 RigidBody::GetRotationEuler() const {
  return imath::QuaternionToEuler(XMLoadFloat4(&rotation_));
}

void RigidBody::SetScale(IVec3 value) {
  scale_ = value;
  collider_->UpdateScaledInertia(mass_, scale_);
  MarkInertiaDirty();
  MarkBoundingDirty();
}

IVec3 RigidBody::GetScale() const { return scale_; }

void RigidBody::SetVelocity(IVec3 value) {
  if (IsStatic()) {
    return;
  }
  velocity_ = value;
}

void RigidBody::SetAcceleration(IVec3 value) {
  if (IsStatic()) {
    return;
  }
  acceleration_ = value;
}

void RigidBody::SetMass(float value) {
  mass_ = value;
  if (mass_ != 0.0f) {
    inv_mass_ = 1.0f / mass_;
  } else {
    inv_mass_ = 0.0f;
  }
}

void RigidBody::SetRestitution(float value) { restitution_ = value; }

void RigidBody::SetRadius(float value) { collider_->SetRadius(value); }

Transform& RigidBody::GetTransform() { return game_object_->GetTransform(); }

IVec3 RigidBody::WorldToLocal(IVec3 world_position) const {
  // 计算逆旋转
  XMVECTOR invRotation = XMQuaternionInverse(XMLoadFloat4(&rotation_));

  // 计算相对于对象的局部位置
  XMVECTOR local_pos = XMLoadFloat3(&world_position) - XMLoadFloat3(&position_);
  local_pos = XMVector3Rotate(local_pos, invRotation);
  local_pos = local_pos / XMLoadFloat3(&scale_);

  IVec3 result;
  XMStoreFloat3(&result, local_pos);
  return result;
}

IVec3 RigidBody::WorldToLocalDirection(IVec3 world_direction) const {
  // 只需应用逆旋转（法线不受平移和缩放影响）
  XMVECTOR invRotation = XMQuaternionInverse(XMLoadFloat4(&rotation_));
  auto local_dir = XMVector3Rotate(XMLoadFloat3(&world_direction), invRotation);
  IVec3 result;
  XMStoreFloat3(&result, local_dir);
  return result;
}

IVec3 RigidBody::LocalToWorld(IVec3 local_position) const {
  // 应用缩放（如果启用）
  XMVECTOR scaled_pos = XMLoadFloat3(&local_position) * XMLoadFloat3(&scale_);
  // 应用旋转
  XMVECTOR rotated_pos = XMVector3Rotate(scaled_pos, XMLoadFloat4(&rotation_));
  // 应用平移
  auto world_pos = rotated_pos + XMLoadFloat3(&position_);
  return StoreF3(world_pos);
}

IVec3 RigidBody::LocalToWorldDirection(IVec3 local_direction) const {
  // 只需应用旋转（方向不受平移和缩放影响）
  auto world_dir =
      XMVector3Rotate(XMLoadFloat3(&local_direction), XMLoadFloat4(&rotation_));
  return StoreF3(world_dir);
}

void RigidBody::AddForce(IVec3 value) {
  if (IsStatic()) {
    return;
  }
  AddF3(forces_, value);
}

void RigidBody::AddTorque(IVec3 value) {
  if (IsStatic()) {
    return;
  }
  AddF3(torques_, value);
}

void RigidBody::AddGravity(IVec3 value) {
  auto f = XMLoadFloat3(&value);
  AddForce(StoreF3(f * mass_));
}

void RigidBody::AddDrag(float k) {
  // 1. 直接使用平方长度避免开方计算
  auto velocity_vec = XMLoadFloat3(&velocity_);
  float speedSq = XMVectorGetX(XMVector3LengthSq(velocity_vec));
  if (speedSq <= FLT_EPSILON) return; // 更精确的零值判断

  // 2. 合并数学运算，只需一次开方
  auto neg_dir = XMVectorReplicate(-k * sqrtf(speedSq));
  XMVECTOR dragForce = XMVectorMultiply(velocity_vec, neg_dir);

  AddForce(StoreF3(dragForce));
}

void RigidBody::AddFriction(float k) {
  auto dir = XMVector3Normalize(XMLoadFloat3(&velocity_));
  // 这里的-k其实就是让dir方向相反，省略了dir * -1。
  AddForce(StoreF3(dir * -k));
}

void RigidBody::ApplyImpulse(IVec3 impulse_f3) {
  if (IsStatic()) return;

  // 限制冲量大小
  auto impulse = XMLoadFloat3(&impulse_f3);
  float impulse_mag = XMVectorGetX(XMVector3Length(impulse));
  if (impulse_mag > max_impulse_) {
    impulse = impulse * (max_impulse_ / impulse_mag);
  }

  // 力都需要与质量相乘
  auto velocity_vec = XMLoadFloat3(&velocity_);
  velocity_vec += impulse * inv_mass_;
  XMStoreFloat3(&velocity_, velocity_vec);

  ClampVelocities();
}

void RigidBody::ApplyImpulse(IVec3 impulse_f3, IVec3 point) {
  // 1. 忽略静态或无限大质量物体
  if (IsStatic()) return;

  // 限制冲量大小
  auto impulse = XMLoadFloat3(&impulse_f3);
  float impulse_mag = XMVectorGetX(XMVector3Length(impulse));
  if (impulse_mag > max_impulse_) {
    impulse = impulse * (max_impulse_ / impulse_mag);
  }

  // 2. 更新线性速度 (Δv = J / m)
  auto velocity_vec = XMLoadFloat3(&velocity_);
  velocity_vec += impulse * inv_mass_;
  XMStoreFloat3(&velocity_, velocity_vec);

  // 3. 更新角速度 (Δω = I⁻¹ · (r × J))
  IVec angular_impulse = XMVector3Cross(XMLoadFloat3(&point), impulse);
  auto angular_vec = XMLoadFloat3(&angular_velocity_);

  auto& world_I = GetInvInertiaWorld();
  angular_vec += XMVector3Transform(angular_impulse, XMLoadFloat3x3(&world_I));
  XMStoreFloat3(&angular_velocity_, angular_vec);

  ClampVelocities();
}

FMat3 RigidBody::GetInvInertiaWorld() {
  if (inertia_dirty_) {
    UpdateInertiaTensor();
  }
  return collider_->inv_inertia_world;
}

void RigidBody::AddSpring(IVec3 anchor, float rest_length, float k) {
  auto force = physics_math::CreateSpringForce(*this, anchor, rest_length, k);
  AddForce(force);
}

void RigidBody::ApplyImpulseLinear(IVec3 value) {
  if (IsStatic()) return;
  auto velocity_vec = XMLoadFloat3(&velocity_);
  velocity_vec += XMLoadFloat3(&value) * inv_mass_;
  XMStoreFloat3(&velocity_, velocity_vec);
}

void RigidBody::ApplyImpulseAngular(IVec3 value) {
  if (IsStatic()) return;
  auto angular_vec = XMLoadFloat3(&angular_velocity_);
  auto& world_I = GetInvInertiaWorld();
  angular_vec +=
      XMVector3Transform(XMLoadFloat3(&value), XMLoadFloat3x3(&world_I));
  XMStoreFloat3(&angular_velocity_, angular_vec);
}

void RigidBody::ClearForces() { forces_ = {0.0f, 0.0f, 0.0f}; }

void RigidBody::ClearTorques() { torques_ = {0.0f, 0.0f, 0.0f}; }

void RigidBody::IntegrateForces(float delta_time) {
  // 如果是静态对象，不处理
  if (IsStatic()) {
    return;
  }

  // 记录上一帧速度（用于变化率限制）
  last_velocity_ = velocity_;
  last_angular_velocity_ = angular_velocity_;

  // 加速度等于力/Mass
  auto force_vec = XMLoadFloat3(&forces_);
  auto acceleration_vec = force_vec * inv_mass_;
  XMStoreFloat3(&acceleration_, acceleration_vec);

  // 计算加速度对速度的影响
  auto velocity_vec = XMLoadFloat3(&velocity_);
  velocity_vec += acceleration_vec * delta_time;
  XMStoreFloat3(&velocity_, velocity_vec);

  // 1. 应用角速度阻尼（0.99~0.995是典型值）
  auto angular_vec = XMLoadFloat3(&angular_velocity_);
  angular_vec *= XMVectorReplicate(angular_damping_);
  XMStoreFloat3(&angular_velocity_, angular_vec);

  MarkBoundingDirty();

  ClearForces();
  ClearTorques();
}

void RigidBody::IntegrateVelocities(float delta_time) {
  if (IsStatic()) return;

  // 1. 计算速度的影响
  auto velocity_vec = XMLoadFloat3(&velocity_);
  auto position_vec = XMLoadFloat3(&position_);
  position_vec += velocity_vec * delta_time;
  XMStoreFloat3(&position_, position_vec);

  // 2. 计算角速度的影响
  auto angular_vec = XMLoadFloat3(&angular_velocity_);
  auto angular_len = imath::len(angular_vec);
  if (angular_len > F_EPSILON) {
    // 计算旋转角度（弧度）和旋转轴
    float angle = angular_len * delta_time;
    XMVECTOR axis = XMVector3Normalize(angular_vec);

    // 创建增量旋转四元数
    XMVECTOR qDelta = XMQuaternionRotationAxis(axis, angle);

    // 应用旋转
    auto quat = XMLoadFloat4(&rotation_);
    quat = XMQuaternionMultiply(quat, qDelta);
    quat = XMQuaternionNormalize(quat);
    XMStoreFloat4(&rotation_, quat);

    // 标记惯性张量需要更新（新增）
    MarkInertiaDirty();
    MarkBoundingDirty();
  }
}

void RigidBody::ClampVelocities() {
  // 限制线速度
  auto velocity_vec = XMLoadFloat3(&velocity_);
  imath::ClampLimit(velocity_vec, max_linear_speed_);

  // 速度变化率限制
  auto last_velocity_vec = XMLoadFloat3(&last_velocity_);
  XMVECTOR delta_v = velocity_vec - last_velocity_vec;
  float delta_v_mag = XMVectorGetX(XMVector3Length(delta_v));
  if (delta_v_mag > max_delta_v_) {
    velocity_vec = last_velocity_vec + XMVector3Normalize(delta_v) *
                                           XMVectorReplicate(max_delta_v_);
  }

  // 保存速度
  XMStoreFloat3(&velocity_, velocity_vec);

  // 限制角速度
  auto angular_vec = XMLoadFloat3(&angular_velocity_);

  // 新增：角速度变化率限制
  auto last_angular_vec = XMLoadFloat3(&last_angular_velocity_);
  XMVECTOR delta_omega = angular_vec - last_angular_vec;
  float delta_omega_mag = XMVectorGetX(XMVector3Length(delta_omega));
  if (delta_omega_mag > max_delta_omega_) {
    angular_vec = last_angular_vec + XMVector3Normalize(delta_omega) *
                                         XMVectorReplicate(max_delta_omega_);
  }

  // 最大角速度限制
  imath::ClampLimit(angular_vec, max_angular_speed_);

  // 保存角速度
  XMStoreFloat3(&angular_velocity_, angular_vec);
}

void RigidBody::MonitorEnergy() {
  float ke = GetKineticEnergy();
  if (ke > max_kinetic_energy_) {
    // 高能状态阻尼
    float damping = exp(-(ke - max_kinetic_energy_) / 1000.0f);
    auto velocity_vec = XMLoadFloat3(&velocity_);
    auto angular_vec = XMLoadFloat3(&angular_velocity_);
    velocity_vec *= damping;
    angular_vec *= damping;
    XMStoreFloat3(&velocity_, velocity_vec);
    XMStoreFloat3(&angular_velocity_, angular_vec);
    KLOG("High energy damped: %.2f > %.2f", ke, max_kinetic_energy_);
  }
}

void RigidBody::LateUpdate(float delta_time) {
  // 将当前的项目设置到GameObject
  auto& t = GetTransform();
  t.position = position_;
  t.rotation = rotation_;
  t.scale = scale_;
  game_object_->UpdateTransform();
}

void RigidBody::BuildLocalInertia() {
  collider_->UpdateScaledInertia(mass_, GetTransform().scale);
  MarkInertiaDirty();
}

void RigidBody::UpdateInertiaTensor() {
  collider_->UpdateWorldInertia(rotation_);
}

bool RigidBody::IsColliding(RigidBody* b, Contact& c) {
  return Collision::IsColliding(this, b, c);
}

const DirectX::BoundingOrientedBox& RigidBody::GetBoundingOrientedBox() const {
  return collider_->GetBoundingOrientedBox(*this);
}

const DirectX::BoundingSphere& RigidBody::GetBoundingSphere() const {
  return collider_->GetBoundingSphere(*this);
}

// 添加到RigidBody类中
const BoundingCapsule& RigidBody::GetBoundingCapsule() const {
  return collider_->GetBoundingCapsule(*this);
}

float RigidBody::GetKineticEnergy() {
  // 平动动能 = 0.5 * m * v²
  auto velocity_vec = XMLoadFloat3(&velocity_);
  float linear_ke =
      0.5f * mass_ * XMVectorGetX(XMVector3LengthSq(velocity_vec));

  // 转动动能 = 0.5 * ω · (I · ω)
  auto angular_vec = XMLoadFloat3(&angular_velocity_);
  float angular_ke = 0.0f;
  if (collider_->type != ShapeType::Particle) {
    auto& world_I = GetInvInertiaWorld();
    XMVECTOR I_omega = XMVector3Transform(
        angular_vec, XMMatrixInverse(nullptr, XMLoadFloat3x3(&world_I)));
    angular_ke = 0.5f * XMVectorGetX(XMVector3Dot(angular_vec, I_omega));
  }

  return linear_ke + angular_ke;
}

} // namespace ifire::game